#include "constants.h"
#include "MyPose.h"
#include "Robot.h"
#include "library.h"
#include "LaserSensor.h"
#include "CostMarker.h"

#ifndef SENSORMODEL_H_
#define SENSORMODEL_H_

class SensorModel {
private:

    MATRIX obstacleCostMap;
    LaserSensor laser;
    CostMarker cm;
	ros::Publisher cost_marker_pub;
    double gaussianProbability(double mean, double variance, double x);

public:
	SensorModel(ros::NodeHandle n);
	virtual ~SensorModel();

	void setObstacleCostMap(const MATRIX &m);
    void estimateWeights(const PoseArray &pa, const Robot &robot, WeightsForPoses &w);

};

#endif /* SENSORMODEL_H_ */
